package com.main;

import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.util.Vector;

import javax.swing.JFrame;

import com.data.Point3D;
import com.data.PolygonMesh;
import com.editors.Editor;
import com.objects.Track;
import com.objects.Train;
import com.objects.Wagon;

public class Railway extends JFrame  {
	
	public int LOC_X=0;
	public int LOC_Y=0;

	public Track[] tracks=null;
	public PolygonMesh landscape=null;
	Vector objects=null;
    
	public int numTracks=30;

	public Train[] trains; 
	
	public double t=0;
	public double dt=.1;

	public void initialize() {


		trains=new Train[1];
		
		trains[0]=new Train("999",3,this);
		trains[0].wagons[0].setWagon_type(1);
		
		tracks=Track.loadRailFromFile(new File("lib/railway.default"));
		
		putTrainOnTheRail(trains[0]);
		
		loadLandscape(new File("lib/railway.default"));
		
		loadObjects(new File("lib/railway.default"));



	}
	

	private void loadObjects(File file) {
	
		try {
			
			BufferedReader br=new BufferedReader(new FileReader(file));
			objects=Editor.loadObjectsFromFile(br);
			br.close();
		
		}catch (Exception e) {
			e.printStackTrace();
		}
		
	}


	private void loadLandscape(File file)  {
		
		try {
		
			BufferedReader br=new BufferedReader(new FileReader(file));
			landscape=Editor.loadPointsFromFile(br,"landscape");
			br.close();
		
		}catch (Exception e) {
			e.printStackTrace();
		}
	}


	public void setLocations(int x, int y) {
		
		for (int i = 0; i < trains.length; i++) {
			
			trains[i].setLocation(x,y);
			trains[i].setVisible(true);
		}
	
		
	}
	

	
	double g=2;
	double gamma=1;


	public void dynamics() {
		

		for (int k = 0; k < trains.length; k++) {	
			
			double traction= trains[k].traction;

			Wagon[] wagons = trains[k].wagons;

			for (int i = 0; i < wagons.length; i++) {

				Wagon w=wagons[i];

				double sinf=Math.sin(w.fi);
				double cosf=Math.cos(w.fi);		

				double ax=0;
				double ay=0;

				double domega=0;

				double sign=w.omega*w.semiLen;

				////front dynamics

				Point3D center_f=new Point3D(w.cm.x+w.semiLen*cosf,w.cm.y+w.semiLen*sinf,0);			


				double dfx=-gamma*(w.vcm.x+sign*(-sinf));
				double dfy=-gamma*(w.vcm.y+sign*(cosf));
				
			

				if(i==0){

					dfx+=traction*cosf;
					dfy+=traction*sinf;
				}
				else{


					Wagon w_prev=wagons[i-1];
					Point3D center_b_prev=w_prev.getBackCenter();

					Point3D link=center_f.substract(center_b_prev);
					double dlink=Point3D.calculateNorm(link);
					if(dlink>0){

						double span=dlink-Wagon.link_distance;
						double core=core(dlink,Wagon.link_distance);
						

						dfx+=(-g*span+core)*link.x/dlink;
						dfy+=(-g*span+core)*link.y/dlink;

					}



				}
				
				
				
				Point3D frontIntersection= w.calculateFrontLinePosition(true);

				if(frontIntersection!=null){


					dfx+=-g*(center_f.x-frontIntersection.x);
					dfy+=-g*(center_f.y-frontIntersection.y);
					
				}

				Point3D vDistance=center_f.substract(w.cm);
				Point3D force=new Point3D(dfx,dfy,0);

				Point3D force_moment=Point3D.calculateCrossProduct(vDistance,force);


				if(force_moment.z>0)
					domega+=Point3D.calculateNorm(force_moment)*w.INVERSE_I;
				else
					domega-=Point3D.calculateNorm(force_moment)*w.INVERSE_I;

				

				ax+=dfx;
				ay+=dfy;

				//////////////////////
				////back dynamics

				Point3D center_b=w.getBackCenter();			



				dfx=-gamma*(w.vcm.x+sign*(sinf));
				dfy=-gamma*(w.vcm.y+sign*(-cosf));



				if(wagons.length>1 && i+1<wagons.length){


					Wagon w_foll=wagons[i+1];
					Point3D center_f_foll=w_foll.getFrontCenter();

					Point3D link=center_b.substract(center_f_foll);
					double dlink=Point3D.calculateNorm(link);
					if(dlink>0){

						double span=dlink-Wagon.link_distance;
						double core=core(dlink,Wagon.link_distance);	
						
						dfx+=(-g*span+core)*link.x/dlink;
						dfy+=(-g*span+core)*link.y/dlink;
						
						//if(i==0)
						//	System.out.println(dlink+" "+core+" "+(-g*span));

					}



				}

				Point3D backIntersection= w.calculateBackLinePosition(true);


				if(backIntersection!=null){


					dfx+=-g*(center_b.x-backIntersection.x);
					dfy+=-g*(center_b.y-backIntersection.y);
				}


				vDistance=center_b.substract(w.cm);
				force=new Point3D(dfx,dfy,0);

				force_moment=Point3D.calculateCrossProduct(vDistance,force);


				if(force_moment.z>0)
					domega+=Point3D.calculateNorm(force_moment)*w.INVERSE_I;
				else
					domega-=Point3D.calculateNorm(force_moment)*w.INVERSE_I;

				ax+=dfx;
				ay+=dfy;

				////////////////

				//System.out.println(domega);

				w.omega+=domega*dt;

				w.vcm.x+=dt*ax;
				w.vcm.y+=dt*ay;

			}
		}

		for (int k = 0; k < trains.length; k++) {			

			Wagon[] wagons = trains[k].wagons;

			for (int i = 0; i < wagons.length; i++) {

				wagons[i].move();

			}	
		}


	}
	



	private double core(double dlink, double link_distance) {
	
		return 0;
		
		//double pow=+10000.0/Math.pow(dlink,2.0);
		//return pow;
	}


	public void putTrainOnTheRailOrig(Train train){
		
		Wagon[] wagons = trains[0].wagons;

		double len=0;

		for(int i=0;i<wagons.length;i++){

			wagons[i].omega=0;

			//double dx=wagons[i].semiLen*2+link_distance;

			wagons[i].cm=new Point3D(len,-200,0);

			wagons[i].vcm=new Point3D(0,0,0);


			len-=wagons[i].semiLen*2+Wagon.link_distance;
		}
	}
	
	
	public void putTrainOnTheRail(Train train){
		
		int index=(int) (Math.random()*numTracks);
		//System.out.println(index);
	
		Track startTrack=tracks[index];
		
		Wagon[] wagons = train.wagons;


		for(int i=0;i<wagons.length;i++){


			Point3D pp1 = null;

			
			if(i>0){
				pp1=wagons[i-1].getBackCenter().clone();
				
				wagons[i].fi=wagons[i-1].fi;
				pp1.translate(
						-Wagon.link_distance*Math.cos(wagons[i].fi),
						-Wagon.link_distance*Math.sin(wagons[i].fi),
				0);
			}
			else{ 
				pp1=startTrack.start.clone();
				wagons[i].fi=startTrack.orientation;
			}
			
		
			wagons[i].cm=pp1.clone();
			wagons[i].cm.translate(
							-wagons[i].semiLen*Math.cos(wagons[i].fi),
							-wagons[i].semiLen*Math.sin(wagons[i].fi),
							0);
			
			
			
			Point3D pp2=wagons[i].calculateBackLinePosition(false);
			
			
			
			if(pp2!=null){
			
				
				/*System.out.println(pp1);
				System.out.println(wagons[i].cm);
				System.out.println(pp2);
				System.out.println();*/
				
				double fi2=Math.atan((pp2.y-pp1.y)/(pp2.x-pp1.x));
					
				//System.out.println(fi2);
				
				if((pp1.y-pp2.y)*fi2<0)
					fi2+=Math.PI;


				wagons[i].fi=fi2;
			
				
				pp1.translate(
						-wagons[i].semiLen*Math.cos(fi2),
						-wagons[i].semiLen*Math.sin(fi2)
						,0);				
				wagons[i].cm=pp1;
			}

			
		}
	}
	

}
